{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import os\n",
    "import glog as log\n",
    "import copy\n",
    "\n",
    "from __future__ import division\n",
    "\n",
    "import open3d as o3d\n",
    "from open3d import JVisualizer\n",
    "import pandas as pd\n",
    "\n",
    "from evaluation.tools.mesh import Mesh\n",
    "from evaluation.tools.mesh_evaluator import MeshEvaluator\n",
    "\n",
    "# Rotation matrices:\n",
    "# East North Up (ENU) frame to Unity's world frame of reference\n",
    "enu_R_unity = np.array([[1, 0, 0],\n",
    "                        [0, 0, 1],\n",
    "                        [0, 1, 0]])\n",
    "unity_R_enu = np.transpose(enu_R_unity)\n",
    "\n",
    "# Right Handed frame to Unity's Left Handed frame of reference\n",
    "righthand_R_lefthand = np.array([[1, 0, 0],\n",
    "                                 [0, -1, 0],\n",
    "                                 [0, 0, 1]])\n",
    "lefthand_R_righthand = np.transpose(righthand_R_lefthand)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# FILL PATHS BELOW\n",
    "#gt_mesh_path = \"/home/tonirv/Downloads/tesse_multiscene_office1_3d_semantic_v5.ply\"\n",
    "#est_mesh_path = \"/home/tonirv/Downloads/tesse_semantics_2.ply\"\n",
    "\n",
    "#gt_mesh_path = \"/home/tonirv/Code/ROS/flight_goggles_ws/src/voxblox/voxblox_ros/mesh_results/semantic_mesh_tonirv_ld_9118_6487309760727328010.ply\"\n",
    "#est_mesh_path = \"/home/tonirv/Code/ROS/flight_goggles_ws/src/voxblox/voxblox_ros/mesh_results/semantic_mesh_tonirv_ld_9118_6487309760727328010.ply\"\n",
    "\n",
    "#gt_mesh_path = \"/home/tonirv/Downloads/tesse_multiscene_office1_3d_semantic_v5.ply\"\n",
    "#est_mesh_path = \"/home/tonirv/Code/ROS/flight_goggles_ws/src/voxblox/voxblox_ros/mesh_results/tesse_semantics_3.ply\"\n",
    "\n",
    "gt_mesh_path = \"/home/tonirv/Downloads/office1_tony.ply\"\n",
    "est_mesh_path = \"/home/tonirv/Code/ROS/flight_goggles_ws/src/voxblox/voxblox_ros/mesh_results/tesse_tonirv_ld_19379_8746799625546883009.ply\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Loading Ground-truth mesh...\")\n",
    "gt_mesh_original = Mesh(gt_mesh_path)\n",
    "print(\"Loading Estimated mesh...\")\n",
    "est_mesh_original = Mesh(est_mesh_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Transform Meshes to same frame of reference\n",
    "gt_mesh = copy.deepcopy(gt_mesh_original)\n",
    "est_mesh = copy.deepcopy(est_mesh_original)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Align Pointclouds Manually:\n",
    "#est_mesh.mesh_o3d.translate([0, -5, 0])\n",
    "#gt_mesh.transform_left(righthand_R_lefthand)\n",
    "gt_mesh.transform_left(enu_R_unity)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vis = o3d.visualization.Visualizer()\n",
    "vis.create_window()\n",
    "vis.get_render_option().mesh_show_back_face = True\n",
    "vis.add_geometry(est_mesh.mesh_o3d)\n",
    "vis.add_geometry(gt_mesh.mesh_o3d)\n",
    "vis.add_geometry(o3d.geometry.create_mesh_coordinate_frame(size=4))\n",
    "vis.run()\n",
    "vis.destroy_window()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "NUMBER_OF_SAMPLES=1000000\n",
    "gt_pcl = o3d.geometry.sample_points_uniformly(gt_mesh.mesh_o3d, NUMBER_OF_SAMPLES)\n",
    "# Don't sample estimated mesh, just pick vertices, otw you'll be mixing colors...\n",
    "# est_pcl = o3d.geometry.sample_points_uniformly(est_mesh.mesh_o3d, NUMBER_OF_SAMPLES)\n",
    "est_pcl = o3d.io.read_point_cloud(est_mesh_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Calculate normals for nice visualization\n",
    "# THIS COLORS THE PCL?>>>>>????\n",
    "#o3d.geometry.estimate_normals(\n",
    "#        est_pcl,\n",
    "#        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1,\n",
    "#                                                          max_nn=5))\n",
    "#o3d.geometry.estimate_normals(\n",
    "#        gt_pcl,\n",
    "#        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1,\n",
    "#                                                          max_nn=5))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vis = o3d.visualization.Visualizer()\n",
    "vis.create_window()\n",
    "vis.get_render_option().mesh_show_back_face = True\n",
    "vis.add_geometry(gt_pcl)\n",
    "vis.add_geometry(est_pcl)\n",
    "vis.run()\n",
    "vis.destroy_window()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# ICP\n",
    "def draw_registration_result(source, target, transformation):\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    target_temp = copy.deepcopy(target)\n",
    "    source_temp.paint_uniform_color([1, 0.706, 0])\n",
    "    target_temp.paint_uniform_color([0, 0.651, 0.929])\n",
    "    source_temp.transform(transformation)\n",
    "    o3d.visualization.draw_geometries([source_temp, target_temp])\n",
    "def draw_correspondences(source, target, correspondences):\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    target_temp = copy.deepcopy(target)\n",
    "    #source_temp.paint_uniform_color([1, 0.706, 0])\n",
    "    #target_temp.paint_uniform_color([0, 0.651, 0.929])\n",
    "    o3d.visualization.draw_geometries([source_temp, #target_temp, \n",
    "                                       correspondences])\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# ICP params\n",
    "ICP_THRESHOLD = 1.5\n",
    "trans_init = np.asarray([[1.0, 0.0, 0.0, 0.0],\n",
    "                         [0.0, 1.0, 0.0, 0.0],\n",
    "                         [0.0, 0.0, 1.0, 0.0],\n",
    "                         [0.0, 0.0, 0.0, 1.0]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualize initial registration problem\n",
    "draw_registration_result(est_pcl, gt_pcl, trans_init)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Evaluate current fit between pointclouds\n",
    "evaluation = o3d.registration.evaluate_registration(est_pcl, gt_pcl, ICP_THRESHOLD, trans_init)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Initial registration\")\n",
    "print(evaluation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Apply point-to-point ICP\")\n",
    "reg_p2p = o3d.registration.registration_icp(\n",
    "    est_pcl, gt_pcl, ICP_THRESHOLD, trans_init,\n",
    "    o3d.registration.TransformationEstimationPointToPoint(),\n",
    "    o3d.registration.ICPConvergenceCriteria(max_iteration = 2000))\n",
    "correspondences = reg_p2p.correspondence_set"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(reg_p2p)\n",
    "print(\"\")\n",
    "\n",
    "print(\"Transformation is:\")\n",
    "print(reg_p2p.transformation)\n",
    "print(\"\")\n",
    "\n",
    "print(\"Correspondence Set:\")\n",
    "print(reg_p2p.correspondence_set)\n",
    "print(\"\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Draw Registration Result\n",
    "draw_registration_result(est_pcl, gt_pcl, reg_p2p.transformation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Draw Only Correspondences\n",
    "c2c_lines = o3d.geometry.create_line_set_from_point_cloud_correspondences(est_pcl, gt_pcl, correspondences)\n",
    "o3d.visualization.draw_geometries([c2c_lines])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Draw PointClouds and Correspondences\n",
    "draw_correspondences(est_pcl, gt_pcl, c2c_lines)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_corresp(est_pcl, gt_pcl, correspondences):\n",
    "    total_negative_matches = 0\n",
    "    total_positive_matches = 0\n",
    "    total_correspondences = len(correspondences)\n",
    "    for correspondence in correspondences:\n",
    "        if np.allclose(est_pcl.colors[correspondence[0]],\n",
    "                       gt_pcl.colors[correspondence[1]]):\n",
    "            total_positive_matches += 1\n",
    "        else:\n",
    "            total_negative_matches += 1\n",
    "\n",
    "    print(\"Positive color matches: \",total_positive_matches)\n",
    "    print(\"Negative color matches: \", total_negative_matches)\n",
    "    print(\"Total correspondences: \", total_correspondences)\n",
    "    assert(total_correspondences == total_negative_matches + total_positive_matches)\n",
    "    print (\"Positive: {}  % \".format(total_positive_matches / total_correspondences * 100))\n",
    "    print (\"Negative: {}  % \".format(total_negative_matches / total_correspondences * 100))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "calc_corresp(est_pcl, gt_pcl, correspondences)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print est_pcl.colors[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import Semantic Labels\n",
    "df = pd.read_csv('/home/tonirv/Code/ROS/flight_goggles_ws/src/voxblox/voxblox_ros/cfg/tesse_multiscene_office1_segmentation_mapping.csv')\n",
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "normalized_df = copy.deepcopy(df)\n",
    "normalized_df['normalized_red'] = df['red'] / 255\n",
    "normalized_df['normalized_green'] = df['green'] / 255\n",
    "normalized_df['normalized_blue'] = df['blue'] / 255\n",
    "\n",
    "print(sum(np.isclose(normalized_df['normalized_red'], gt_pcl.colors[0][0])))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from hashlib import sha1\n",
    "\n",
    "from numpy import all, array, uint8\n",
    "\n",
    "\n",
    "class hashable(object):\n",
    "    r'''Hashable wrapper for ndarray objects.\n",
    "        Instances of ndarray are not hashable, meaning they cannot be added to\n",
    "        sets, nor used as keys in dictionaries. This is by design - ndarray\n",
    "        objects are mutable, and therefore cannot reliably implement the\n",
    "        __hash__() method.\n",
    "        The hashable class allows a way around this limitation. It implements\n",
    "        the required methods for hashable objects in terms of an encapsulated\n",
    "        ndarray object. This can be either a copied instance (which is safer)\n",
    "        or the original object (which requires the user to be careful enough\n",
    "        not to modify it).\n",
    "    '''\n",
    "    def __init__(self, wrapped, tight=False):\n",
    "        r'''Creates a new hashable object encapsulating an ndarray.\n",
    "            wrapped\n",
    "                The wrapped ndarray.\n",
    "            tight\n",
    "                Optional. If True, a copy of the input ndaray is created.\n",
    "                Defaults to False.\n",
    "        '''\n",
    "        self.__tight = tight\n",
    "        self.__wrapped = array(wrapped) if tight else wrapped\n",
    "        self.__hash = int(sha1(wrapped.view(uint8)).hexdigest(), 16)\n",
    "\n",
    "    def __eq__(self, other):\n",
    "        return all(self.__wrapped == other.__wrapped)\n",
    "\n",
    "    def __hash__(self):\n",
    "        return self.__hash\n",
    "\n",
    "    def unwrap(self):\n",
    "        r'''Returns the encapsulated ndarray.\n",
    "            If the wrapper is \"tight\", a copy of the encapsulated ndarray is\n",
    "            returned. Otherwise, the encapsulated ndarray itself is returned.\n",
    "        '''\n",
    "        if self.__tight:\n",
    "            return array(self.__wrapped)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# No need to hash, we have ids now\n",
    "red_col = df['red'].to_numpy()\n",
    "blue_col = df['blue'].to_numpy()\n",
    "green_col = df['green'].to_numpy()\n",
    "\n",
    "def f(x):    \n",
    "    return str(x['normalized_red'])+str(x['normalized_green'])+str(x['normalized_blue'])\n",
    "\n",
    "hashed_df = copy.deepcopy(normalized_df)\n",
    "hashed_df['hash'] = hashed_df.apply(f, axis=1)\n",
    "hashed_df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate table from color to id.\n",
    "def label_from_color(color):\n",
    "    norm_r = color[0]\n",
    "    norm_g = color[1]\n",
    "    norm_b = color[2]\n",
    "    # TODO(Toni): can be greatly optimized... TO SLOW NOW\n",
    "    # TODO(Toni): you are comparing floats with == ......\n",
    "    return normalized_df.loc[(normalized_df['normalized_red'] == norm_r) & \n",
    "                  (normalized_df['normalized_green'] == norm_g) & \n",
    "                  (normalized_df['normalized_blue'] == norm_b)]['id'].unique().tolist()[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compare labels between correspondences:\n",
    "total_label_correspondences = {i:0 for i in normalized_df['id'].unique()}\n",
    "# Initialize dictionaries to 0:\n",
    "total_positive_matches = {i:0 for i in normalized_df['id'].unique()}\n",
    "total_negative_matches = copy.deepcopy(total_positive_matches)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_per_label_corresp():\n",
    "    for correspondence in correspondences:\n",
    "        est_pcl_color = est_pcl.colors[correspondence[0]]\n",
    "        color_hash = str(est_pcl_color[0])+str(est_pcl_color[1])+str(est_pcl_color[2])\n",
    "        label_id = label_from_color(est_pcl_color)\n",
    "        if np.allclose(est_pcl_color, gt_pcl.colors[correspondence[1]]):\n",
    "            total_positive_matches[label_id] += 1\n",
    "        else:\n",
    "            total_negative_matches[label_id] += 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "calc_per_label_corresp()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print \"Dict for total_positive_matches: \\n\", total_positive_matches\n",
    "print \"Dict for total_negative_matches: \\n\", total_negative_matches\n",
    "for label_id in df['id'].unique().tolist():\n",
    "    total_label_correspondences[label_id] = (total_positive_matches[label_id] + total_negative_matches[label_id])\n",
    "print \"Dict for total_label_correspondences: \\n\", total_label_correspondences\n",
    "for label_id in df['id'].unique().tolist():\n",
    "    print (\"ID: {}\".format(label_id))\n",
    "    print (\"Positive: {}  % \".format(total_positive_matches[label_id] / total_label_correspondences[label_id] * 100))\n",
    "    print (\"Negative: {}  % \".format(total_negative_matches[label_id] / total_label_correspondences[label_id] * 100))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.15rc1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
